/*
 * @Author: bor_fish 1219854025@qq.com
 * @Date: 2025-05-15 16:35:00
 * @LastEditors: bor_fish 1219854025@qq.com
 * @LastEditTime: 2025-05-26 16:31:19
 * @FilePath: \SendCAN\actuator.c
 * @Description: CAN通信程序 - 驱动器接收和反馈
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <net/if.h>
#include <time.h>
#include <sys/select.h>
#include <sys/time.h>
#include <fcntl.h>

// 定义常量
#define BASE_CAN_ID 0x320000        // 基础ID为320000
#define MIN_DEVICE_ID 2		        // X的范围
#define MAX_DEVICE_ID 8		        // X的范围
#define RESPONSE_ID_SUFFIX_22 0x22  // 自反馈帧ID后缀
#define RESPONSE_ID_SUFFIX_23 0x23  // 自反馈帧ID后缀
#define RESPONSE_ID_SUFFIX_24 0x24  // 自反馈帧ID后缀

// 定义参数范围
#define MIN_VOLTAGE 0				//母线电压
#define MAX_VOLTAGE 400				//母线电压
#define MIN_CURRENT 0				//母线电流
#define MAX_CURRENT 255				//母线电流
#define MIN_TEMPERATURE 0			//电机温度
#define MAX_TEMPERATURE 150			//电机温度
#define MIN_LEVEL 0					//补偿器液位信息与伸缩机构状态
#define MAX_LEVEL 100				//补偿器液位信息与伸缩机构状态
#define MIN_VERSION 0				//版本号
#define MAX_VERSION 255				//版本号

// 异常模拟参数
#define RECEIVE_FAILURE_RATE 0.1    // 接收失败概率 10%
#define FEEDBACK_FAILURE_RATE 0.1   // 反馈发送失败概率 10%
#define STATUS_FAILURE_RATE 0.1     // 状态数据发送失败概率 10%
#define STATUS_DELAY_MIN 0          // 最小状态数据发送延迟 0ms
#define STATUS_DELAY_MAX 5000     // 最大状态数据发送延迟 5ms

// 增加CAN数据错误模拟概率
#define ID_ERROR_RATE 0.001         // CAN的ID错误概率 0.2%
#define DLC_ERROR_RATE 0.001        // CAN的DLC错误概率 0.2%
#define DATA_ERROR_RATE 0.002       // CAN的数据错误概率 0.2%
#define CRC_ERROR_RATE 0.005        // CAN的CRC错误概率 0.2%
// 生成指定范围内的随机数
int get_random_value(int min, int max) {
    return min + (rand() % (max - min + 1));
}

// 模拟接收失败
bool simulate_receive_failure() {
    return (rand() % 100) < (RECEIVE_FAILURE_RATE * 100);
}

// 模拟反馈发送失败
bool simulate_feedback_failure() {
    return (rand() % 100) < (FEEDBACK_FAILURE_RATE * 100);
}

// 模拟状态数据发送失败
bool simulate_status_failure() {
    return (rand() % 100) < (STATUS_FAILURE_RATE * 100);
}

// 模拟状态数据发送延迟
void simulate_status_delay() {
    int delay = get_random_value(STATUS_DELAY_MIN, STATUS_DELAY_MAX);
    usleep(delay);
}

void inject_frame_error(struct can_frame *frame) {
    // 帧ID错误
    if ((rand() % 1000) < (ID_ERROR_RATE * 1000)) {
        frame->can_id ^= get_random_value(1, 0x7FF); // 随机扰动ID
        printf("模拟帧ID错误\n");
    }
    // DLC错误
    if ((rand() % 1000) < (DLC_ERROR_RATE * 1000)) {
        frame->can_dlc = get_random_value(1, 8); // 随机DLC
        printf("模拟DLC错误\n");
    }
    // 数据字段错误
    // if ((rand() % 1000) < (DATA_ERROR_RATE * 1000)) {
    //     for (int i = 0; i < 8; i++) {
    //         frame->data[i] = rand() & 0xFF;
    //     }
    //     printf("模拟数据字段错误\n");
    // }
    // CRC错误（用数据损坏模拟）
    if ((rand() % 1000) < (CRC_ERROR_RATE * 1000)) {
        frame->data[get_random_value(0, 7)] ^= 0xFF; // 随机字节取反
        printf("模拟CRC错误（数据损坏）\n");
    }
}

int main(int argc, char *argv[])
{
    if (argc < 2) {
        printf("用法: %s actuator\n", argv[0]);
        return 1;
    }

    struct ifreq ifr = {0};
    struct sockaddr_can can_addr = {0};
    int sockfd = -1;
    int ret;

    // 初始化随机数种子
    srand(time(NULL));

    // 打开套接字
    sockfd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if (0 > sockfd) {
        perror("socket error");
        exit(EXIT_FAILURE);
    }

    // 指定can0设备
    strcpy(ifr.ifr_name, "vcan0");
    ioctl(sockfd, SIOCGIFINDEX, &ifr);
    can_addr.can_family = AF_CAN;
    can_addr.can_ifindex = ifr.ifr_ifindex;

    // 绑定
    ret = bind(sockfd, (struct sockaddr *)&can_addr, sizeof(can_addr));
    if (0 > ret) {
        perror("bind error");
        close(sockfd);
        exit(EXIT_FAILURE);
    }

    if (strcmp(argv[1], "actuator") == 0) {

        struct can_frame rx_frame = {0};
        struct can_frame tx_frame = {0};
        tx_frame.can_dlc = 8;  // 固定8字节数据长度

        // 存储每个设备的当前速度
        int16_t device_speeds[MAX_DEVICE_ID + 1] = {0};

        // 设置套接字为非阻塞模式
        int flags = fcntl(sockfd, F_GETFL, 0);
        fcntl(sockfd, F_SETFL, flags | O_NONBLOCK);

        // 用于select的文件描述符集合
        fd_set readfds;
        struct timeval tv;
        tv.tv_sec = 0;
        tv.tv_usec = 100000;  // 100ms超时

        // 上次发送状态数据的时间
        struct timeval last_send_time;
        gettimeofday(&last_send_time, NULL);

        while (1) {
            // 设置select超时
            tv.tv_sec = 0;
            tv.tv_usec = 100000;  // 100ms

            FD_ZERO(&readfds);
            FD_SET(sockfd, &readfds);

            // 等待数据或超时
            int ret = select(sockfd + 1, &readfds, NULL, NULL, &tv);
            
            if (ret > 0) {
                // 有数据可读
                int nbytes = read(sockfd, &rx_frame, sizeof(rx_frame));
                if (nbytes > 0) {
                    // 模拟接收失败
                    if (simulate_receive_failure()) {
                        printf("设备 0x%X 接收失败\n", rx_frame.can_id);
                        continue;
                    }

                    // 提取设备ID
                    int device_id = (rx_frame.can_id >> 4) & 0xF;
                    
                    // 设置反馈ID (320X2格式)
                    tx_frame.can_id = 0x32000 | (device_id << 4) | 0x2;
                                   
                    // 复制接收到的数据到反馈帧
                    memcpy(tx_frame.data, rx_frame.data, 8);

                    // 保存接收到的速度值
                    device_speeds[device_id] = (rx_frame.data[1] | (rx_frame.data[2] << 8));

                    // 模拟反馈发送失败
                    if (!simulate_feedback_failure()) {
                        // 注入错误
                        inject_frame_error(&tx_frame);

                        // 发送反馈
                        ret = write(sockfd, &tx_frame, sizeof(tx_frame));
                        if (ret != sizeof(tx_frame)) {
                            perror("write feedback error");
                            break;
                        }

                        // 打印接收和发送的数据
                        printf("收到设备 0x%X 的数据: ", rx_frame.can_id);
                        for (int i = 0; i < 8; i++) {
                            printf("%02X ", rx_frame.data[i]);
                        }
                        printf("\n发送反馈到 0x%X\n", tx_frame.can_id);
                    } else {
                        printf("设备 0x%X 反馈发送失败\n", tx_frame.can_id);
                    }
                }
            }

            // 检查是否需要发送状态数据
            struct timeval current_time;
            gettimeofday(&current_time, NULL);
            long time_diff = (current_time.tv_sec - last_send_time.tv_sec) * 1000000L + 
                           (current_time.tv_usec - last_send_time.tv_usec);

            if (time_diff >= 100000) {  // 100ms
                // 模拟状态数据发送延迟
                simulate_status_delay();

                // 发送320X22格式的状态数据
                for (int device_id = MIN_DEVICE_ID; device_id <= MAX_DEVICE_ID; device_id++) {
                    // 模拟状态数据发送失败
                    if (simulate_status_failure()) {
                        printf("设备 0x%X 状态数据发送失败\n", BASE_CAN_ID | (device_id << 8) | RESPONSE_ID_SUFFIX_22);
                        continue;
                    }

                    struct can_frame status_frame = {0};
                    status_frame.can_dlc = 8;
                    status_frame.can_id = BASE_CAN_ID | (device_id << 8) | RESPONSE_ID_SUFFIX_22;
                    
                    // 生成随机数据
                    status_frame.data[0] = get_random_value(0, 1);  // 急停信号状态
                    
                    // 使用保存的速度值
                    status_frame.data[1] = device_speeds[device_id] & 0xFF;        // 转速低位
                    status_frame.data[2] = (device_speeds[device_id] >> 8) & 0xFF; // 转速高位
                    
                    // 母线电压
                    uint16_t voltage = get_random_value(MIN_VOLTAGE, MAX_VOLTAGE);
                    status_frame.data[3] = voltage & 0xFF;         // 电压低位
                    status_frame.data[4] = (voltage >> 8) & 0xFF;  // 电压高位
                    
                    // 母线电流
                    status_frame.data[5] = get_random_value(MIN_CURRENT, MAX_CURRENT);
                    
                    // 电机温度
                    status_frame.data[6] = get_random_value(MIN_TEMPERATURE, MAX_TEMPERATURE);
                    
                    // 驱动器温度
                    status_frame.data[7] = get_random_value(MIN_TEMPERATURE, MAX_TEMPERATURE);
                    // 注入错误
                    inject_frame_error(&status_frame);

                    // 发送状态数据
                    ret = write(sockfd, &status_frame, sizeof(status_frame));
                    if (ret != sizeof(status_frame)) {
                        perror("write status error");
                        break;
                    }

                    // 发送320X23数据
                    if (!simulate_status_failure()) {
                        struct can_frame status_frame23 = {0};
                        status_frame23.can_dlc = 8;
                        status_frame23.can_id = BASE_CAN_ID | (device_id << 8) | RESPONSE_ID_SUFFIX_23;

                        // 填充320X23数据
                        status_frame23.data[0] = get_random_value(MIN_LEVEL, MAX_LEVEL);  // 补偿器液位信息
                        status_frame23.data[1] = get_random_value(MIN_LEVEL, MAX_LEVEL);  // 伸缩机构状态
                        
                        // 接近开关状态（随机生成）
                        status_frame23.data[2] = rand() & 0x0F;  // 只使用低4位
                        
                        // 故障位（随机生成）
                        status_frame23.data[3] = rand() & 0xFF;  // 故障位-1
                        status_frame23.data[4] = rand() & 0xFF;  // 故障位-2
                        
                        // 备用字节
                        status_frame23.data[5] = 0;
                        status_frame23.data[6] = 0;
                        status_frame23.data[7] = 0;
                        // 注入错误
                        inject_frame_error(&status_frame);

                        // 发送320X23数据
                        ret = write(sockfd, &status_frame23, sizeof(status_frame23));
                        if (ret != sizeof(status_frame23)) {
                            perror("write status23 error");
                            break;
                        }
                    } else {
                        printf("设备 0x%X 320X23数据发送失败\n", BASE_CAN_ID | (device_id << 8) | RESPONSE_ID_SUFFIX_23);
                    }

                    // 发送320X24数据
                    if (!simulate_status_failure()) {
                        struct can_frame status_frame24 = {0};
                        status_frame24.can_dlc = 8;
                        status_frame24.can_id = BASE_CAN_ID | (device_id << 8) | RESPONSE_ID_SUFFIX_24;

                        // 填充320X24数据
                        status_frame24.data[0] = get_random_value(MIN_VERSION, MAX_VERSION);  // 硬件版本号
                        status_frame24.data[1] = get_random_value(MIN_VERSION, MAX_VERSION);  // 软件版本号
                        
                        // 备用字节
                        for (int i = 2; i < 8; i++) {
                            status_frame24.data[i] = 0;
                        }
                        // 注入错误
                        inject_frame_error(&status_frame);

                        // 发送320X24数据
                        ret = write(sockfd, &status_frame24, sizeof(status_frame24));
                        if (ret != sizeof(status_frame24)) {
                            perror("write status24 error");
                            break;
                        }
                    } else {
                        printf("设备 0x%X 320X24数据发送失败\n", BASE_CAN_ID | (device_id << 8) | RESPONSE_ID_SUFFIX_24);
                    }
                }
                
                // 更新上次发送时间
                gettimeofday(&last_send_time, NULL);
            }
        }
    } else {
        printf("未知模式: %s\n", argv[1]);
    }

    close(sockfd);
    return 0;
} 
